// Class include
#include "CSerialReader.hpp"

// System includes
#include <unistd.h>     // For sleep
#include <string.h>
#include <stdio.h>

// Other includes
#include "CSerialPort.hpp"
#include "CWatchdog.hpp"
#include "CTrace.hpp"
#include "Types.hpp"

// Forward declaration includes
#include "CCommand.hpp"
#include "CObserver.hpp"

CSerialReader::CSerialReader(const char* device, CObserver* pObserver)
: CObservable()
, mDevice(device)
, mpSerial(0)
{
   addObserver(pObserver);
   addObserver(CWatchdog::getInstance());
}

CSerialReader::~CSerialReader()
{
}

void CSerialReader::initSerialPort()
{
   if (0 != mpSerial)
   {
      delete mpSerial;
      mpSerial = 0;
   }
   
   CSerialPort* serial = new CSerialPort(mDevice);
   if (false == serial->isOpened())
   {
      TRC_ERROR("Could not open device: %s", mDevice);
      return;
   }

   mpSerial = serial;
}

void CSerialReader::start()
{
   initSerialPort();

   while(true)
   {
      unsigned char* pBuf = new unsigned char[COMMAND_LENGTH];
      unsigned int bytesRead = mpSerial->recv(pBuf, COMMAND_LENGTH);

      /*
      // This code does not work due to read() never returns -1
      if (bytesRead < 0)
      {
         TRC_WARNING("An error occured when reading from Serial port. Trying to reconnect after 1 second");
         usleep( 1 * 1000 * 1000 );  // wait 1 sec try again
         
         initSerialPort();
      }
      */
      
      if (bytesRead <= 0) continue;
      
      if (bytesRead != COMMAND_LENGTH)
      {
         TRC_WARNING("Something is wrong while reading command from socket. Expected %d bytes, but read %d bytes", COMMAND_LENGTH, bytesRead);
         for (int i=0; i<bytesRead; i++) printf("[%d] = %d ", i, pBuf[i]); printf("\n");  // Debug output
         continue;
      }

      // TODO: create command object. Put command to queue
      CCommand cmd(COMMAND_SOURCE_ARDUINO, pBuf);
      TRC_DEBUG("Read command from Arduino: %s, value=%d", toString(cmd.getType()), cmd.getValue32());
      for (int i=0; i<bytesRead; i++) printf("[%d] = %d ", i, pBuf[i]); printf("\n");  // Debug output

      if (COMMAND_INVALID != cmd.getType())
      {
         notifyObservers(cmd);
      }

      delete[] pBuf;
   }
}

void CSerialReader::sendCommand(CCommand cmd)
{
   if (false == isConnected())
   {
      TRC_WARNING("Arduino is closed. Skipping command from Socket to Arduino");
      return;
   }

   if (0 == mpSerial)
   {
      TRC_ERROR("Serial port is NULL. Something is wrong");
      return;
   }

   unsigned char* pRawBuffer = new unsigned char[COMMAND_LENGTH];
   cmd.getRawBuffer(pRawBuffer);

   unsigned int bytesSent = mpSerial->send(pRawBuffer, COMMAND_LENGTH);
   if (bytesSent != COMMAND_LENGTH)
   {
      TRC_WARNING("Not all bytes where sent: %d instead of %d", bytesSent, COMMAND_LENGTH);
   }

   delete[] pRawBuffer;
}

bool CSerialReader::isConnected()
{
   // Assuming that arduino is always connected
   return true;
}